//
// Created by bourne on 2024/4/23.
//

#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <Eigen/Eigen>

// 全局发布者
ros::Publisher marker_pub;

void addMsg(visualization_msgs::Marker& mk, const Eigen::Vector3d& pos, const double& scale) {
  // 设置Marker的基本信息
  mk.header.frame_id = "world";
  mk.header.stamp = ros::Time::now();

//  mk.action = visualization_msgs::Marker::ADD;
//  mk.id = 0; // 标记ID
//  mk.ns = "basic_shapes"; // 命名空间
//  mk.type = visualization_msgs::Marker::SPHERE; // 标记类型
  mk.pose.position.x = pos[0];
  mk.pose.position.y = pos[1];
  mk.pose.position.z = pos[2];

  mk.scale.x = scale;
  mk.scale.y = scale;
  mk.scale.z = scale;

  mk.color.r = 1.0;
  mk.color.g = 0.0;
  mk.color.b = 0.0;
  mk.color.a = 1.0;
}


// 测试函数
void testMarkerActions() {
  ros::Rate rate(1.0); // 设置循环频率为1Hz

  while (ros::ok()) {
    // 创建一个新的Marker对象
    visualization_msgs::Marker mk1;
    visualization_msgs::Marker mk2;
    visualization_msgs::Marker mk3;
    visualization_msgs::Marker mk4;

    Eigen::Vector3d pos1 (0.0, 0.0, 0.0) ;
    Eigen::Vector3d pos2 (2.0, 2.0, 0.0);

    Eigen::Vector3d pos3 (5.0, 0.0, 0.0);
    Eigen::Vector3d pos4 (7.0, 2.0 ,0.0);

    ros::Duration(1.0).sleep();

    addMsg(mk1, pos1, 0.3);
    mk1.action = visualization_msgs::Marker::ADD;
    mk1.id = 0; // 标记ID
    mk1.ns = "namespace1"; // 命名空间
    mk1.type = visualization_msgs::Marker::SPHERE; // 标记类型
    marker_pub.publish(mk1);
    ros::Duration(0.001).sleep();


    addMsg(mk2, pos2, 0.3);
    mk2.action = visualization_msgs::Marker::ADD;
    mk2.id = 1; // 标记ID
    mk2.ns = "namespace1"; // 命名空间
    mk2.type = visualization_msgs::Marker::SPHERE; // 标记类型
    marker_pub.publish(mk2);
    ros::Duration(0.001).sleep();
    std::cout << "pub pt1 pt2" << std::endl;

//    ros::Duration(2.0).sleep();
//    mk1.action = visualization_msgs::Marker::DELETE;
//    marker_pub.publish(mk1);
//    std::cout << "delete pt1" << std::endl;
//
//    ros::Duration(2.0).sleep();
//    mk2.id = 3;
//    mk2.action = visualization_msgs::Marker::DELETE;
//    marker_pub.publish(mk2);
//    std::cout << "delete pt2" << std::endl;


    ros::Duration(2.0).sleep();
    addMsg(mk3, pos3, 0.5);
    mk3.action = visualization_msgs::Marker::ADD;
    mk3.id = 0; // 标记ID
    mk3.ns = "namespace2"; // 命名空间
    mk3.type = visualization_msgs::Marker::SPHERE; // 标记类型
    marker_pub.publish(mk3);
    ros::Duration(1).sleep();

    addMsg(mk4, pos4, 0.5);
    mk4.action = visualization_msgs::Marker::ADD;
    mk4.id = 0; // 标记ID
    mk4.ns = "namespace2"; // 命名空间
    mk4.type = visualization_msgs::Marker::SPHERE; // 标记类型
    marker_pub.publish(mk4);
    ros::Duration(0.001).sleep();

    std::cout << "pub pt3 pt4" << std::endl;

    // 现在这么用，delete all有一些bug，
    // 本来应该是只删除该ns下的，但是实际上却删除了所有的标记
    ros::Duration(2.0).sleep();
    mk3.action = visualization_msgs::Marker::DELETEALL;
    marker_pub.publish(mk3);
    std::cout << "delete pt3 pt4" << std::endl;

    ros::Duration(2.0).sleep();
    // 保持事件循环，处理回调等
    ros::spinOnce();
//    rate.sleep();
  }
}
int main(int argc, char** argv) {
  ros::init(argc, argv, "marker_action_test");
  ros::NodeHandle nh("~");

  // 创建一个发布者，用于发布Marker消息
  marker_pub = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 10);

  // 调用测试函数
  testMarkerActions();

  return 0;
}